#ifndef HANDTRACKING_H
#define HANDTRACKING_H

#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
//#include <pcl/segmentation/region_growing_rgb.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui_c.h"

#include <iostream>
#include <stdio.h>

using namespace cv;
using namespace pcl::visualization;
using namespace std;

class Handtracking
{

 Mat Frames;
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr nube;

public:
  int j;
  Handtracking()
  {
  };
  ~Handtracking()
  {};

  /*void Search_segmentation(pcl::PointCloud<pcl::PointXYZRGB>::Ptr nube)
  {
   pcl::search::Search <pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
  pcl::IndicesPtr indices (new std::vector <int>);
  pcl::PassThrough<pcl::PointXYZRGB> pass;
  pass.setInputCloud (nube);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.0);
  pass.filter (*indices);

  pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
  reg.setInputCloud (nube);
  reg.setIndices (indices);
  reg.setSearchMethod (tree);
  reg.setDistanceThreshold (10);
  reg.setPointColorThreshold (6);
  reg.setRegionColorThreshold (5);
  reg.setMinClusterSize (600);

  std::vector <pcl::PointIndices> clusters;
  reg.extract (clusters);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
  CloudViewer viewer ("Cluster viewer");
  viewer.showCloud (colored_cloud);
  while (!viewer.wasStopped ()){}
  

  }*/


  void HandPixel(Mat depth){
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr nube(new pcl::PointCloud<pcl::PointXYZRGB>);
  CloudViewer viewer("Cloud Viewer");
  Mat Ultima =Frames;
    nube->width = Ultima.cols;
    nube->height = Ultima.rows;
    for(int i = 0; i < Ultima.rows-1; i++)
      {
      for(int j = 0; j < Ultima.cols-1; j++)
        {
       cv::Vec3b& bgr = Ultima.at<cv::Vec3b>(i,j);
       pcl::PointXYZRGB p;
       p.r = bgr[2];
       p.g = bgr[1];
       p.b = bgr[0];
       p.x=j;
       p.z=i;
       cv::Vec3b& dep = depth.at<cv::Vec3b>(i,j);
       p.y= (dep[0]+dep[1]+dep[2]);
       if((p.y<300) & (p.r>95) & (p.g>40) & (p.b>20)& (abs(p.r-p.g) >15) & (p.r>p.g) & (p.g>p.b) )
       nube->push_back(p);
      }
    }
       cout << "Bucle Acabado"<< endl;
   //Search_segmentation(nube);
  viewer.showCloud (nube);
   while (!viewer.wasStopped()){}

  };

  void anadir_frame (Mat source)
  {
   Frames=source;
  };


/*
  Mat HandSegmentacion (Mat source);

  Mat HandForm(Mat source);

  Mat HandTracking();
*/
};
#endif
